
#include <plan_manage/planner_manager.h>
#include <exploration_manager/fast_exploration_manager.h>
#include <traj_utils/planning_visualization.h>

#include <exploration_manager/fast_exploration_fsm.h>
#include <exploration_manager/expl_data.h>
#include <exploration_manager/HGrid.h>
#include <exploration_manager/GridTour.h>

#include <plan_env/edt_environment.h>
#include <plan_env/sdf_map.h>
#include <plan_env/multi_map_manager.h>
#include <active_perception/perception_utils.h>
#include <active_perception/hgrid.h>
// #include <active_perception/uniform_grid.h>
// #include <lkh_tsp_solver/lkh_interface.h>
// #include <lkh_mtsp_solver/lkh3_interface.h>

#include <fstream>

using Eigen::Vector4d;

namespace fast_planner {
void FastExplorationFSM::init(ros::NodeHandle& nh) {
  fp_.reset(new FSMParam);
  fd_.reset(new FSMData);

  /*  Fsm param  */
  /*
    阈值（thresh_replan1, thresh_replan2, thresh_replan3） 用于控制何时触发重新规划。
    replan_time 控制重新规划的最大执行时间。
    attempt_interval 控制每次重新规划尝试之间的间隔时间。
    pair_opt_interval 控制配对优化的时间间隔。
    repeat_send_num 控制重新发送的次数。
  */
  nh.param("fsm/thresh_replan1", fp_->replan_thresh1_, -1.0);
  nh.param("fsm/thresh_replan2", fp_->replan_thresh2_, -1.0);
  nh.param("fsm/thresh_replan3", fp_->replan_thresh3_, -1.0);
  nh.param("fsm/replan_time", fp_->replan_time_, -1.0);
  nh.param("fsm/attempt_interval", fp_->attempt_interval_, 0.2);
  nh.param("fsm/pair_opt_interval", fp_->pair_opt_interval_, 1.0);
  nh.param("fsm/repeat_send_num", fp_->repeat_send_num_, 10);
  nh.param("fsm/collision_dis1", collision_dis1_, 1.7);
  nh.param("fsm/collision_dis2", collision_dis2_, 1.3);
  nh.param("fsm/angle1", angle1, 45.0);
  nh.param("fsm/angle2", angle2, 60.0);

  nh.param("fsm/end_diff", end_diff_, 25.0);
  nh.param("fsm/land_ins", land_instantly, true);
  nh.param("sdf_map/box_min_x", x_min_, 0.0);
  nh.param("sdf_map/box_max_x", x_max_, 12.0);
  nh.param("sdf_map/box_min_y", y_min_, 0.0);
  nh.param("sdf_map/box_max_y", y_max_, 30.0);

  /* Initialize main modules */
  expl_manager_.reset(new FastExplorationManager);
  expl_manager_->initialize(nh);
  visualization_.reset(new PlanningVisualization(nh));

  planner_manager_ = expl_manager_->planner_manager_;
  state_ = EXPL_STATE::INIT;
  fd_->have_odom_ = false;
  fd_->state_str_ = { "INIT", "WAIT_TRIGGER", "PLAN_TRAJ", "PUB_TRAJ", "EXEC_TRAJ", "FINISH",
    "IDLE", "URGENT_STOP", "PLAN_TRAJ_CA", "PUB_TRAJ_CA", "EXEC_TRAJ_CA" };
  fd_->static_state_ = true;
  fd_->trigger_ = false;
  fd_->avoid_collision_ = false;
  fd_->go_back_ = false;
  may_collision = false;
  may_replan = false;
  time_begin = false;
  ftr_count_ = 0;

  /* Ros sub, pub and timer */
  exec_timer_ = nh.createTimer(ros::Duration(0.01), &FastExplorationFSM::FSMCallback, this);
  safety_timer_ = nh.createTimer(ros::Duration(0.05), &FastExplorationFSM::safetyCallback, this);
  frontier_timer_ =
      nh.createTimer(ros::Duration(0.05), &FastExplorationFSM::frontierCallback, this);
  collision_timer_ =
      nh.createTimer(ros::Duration(0.05), &FastExplorationFSM::collisionCallback, this);
  count_timer_ = nh.createTimer(ros::Duration(0.05), &FastExplorationFSM::countCallback, this);
  // env_timer_ = nh.createTimer(ros::Duration(2.0), &FastExplorationFSM::envCallback, this);

  trigger_sub_ = nh.subscribe("/clicked_point", 1, &FastExplorationFSM::triggerCallback, this);
  // trigger_sub_ = nh.subscribe("/move_base_simple/goal", 1, &FastExplorationFSM::triggerCallback,
  // this);
  odom_sub_ = nh.subscribe("/odom_world", 1, &FastExplorationFSM::odometryCallback, this);

  replan_pub_ = nh.advertise<std_msgs::Empty>("/planning/replan", 10);
  new_pub_ = nh.advertise<std_msgs::Empty>("/planning/new", 10);
  bspline_pub_ = nh.advertise<bspline::Bspline>("/planning/bspline", 10);
  stop_pub_ = nh.advertise<std_msgs::Bool>("/fucking_stop", 10);
  land_pub_ = nh.advertise<std_msgs::String>("/xtdrone/iris/cmd", 10);
  radio_land_pub_ = nh.advertise<std_msgs::String>("/land", 10);
  end_sub_ = nh.subscribe("/land", 10, &FastExplorationFSM::endCallback, this);

  // Swarm, timer, pub and sub
  drone_state_timer_ =
      nh.createTimer(ros::Duration(0.04), &FastExplorationFSM::droneStateTimerCallback, this);
  drone_state_pub_ =
      nh.advertise<exploration_manager::DroneState>("/swarm_expl/drone_state_send", 10);
  drone_state_sub_ = nh.subscribe(
      "/swarm_expl/drone_state_recv", 10, &FastExplorationFSM::droneStateMsgCallback, this);

  opt_timer_ = nh.createTimer(ros::Duration(0.05), &FastExplorationFSM::optTimerCallback, this);
  opt_pub_ = nh.advertise<exploration_manager::PairOpt>("/swarm_expl/pair_opt_send", 10);
  opt_sub_ = nh.subscribe("/swarm_expl/pair_opt_recv", 100, &FastExplorationFSM::optMsgCallback,
      this, ros::TransportHints().tcpNoDelay());
  opt_res_pub_ =
      nh.advertise<exploration_manager::PairOptResponse>("/swarm_expl/pair_opt_res_send", 10);
  opt_res_sub_ = nh.subscribe("/swarm_expl/pair_opt_res_recv", 100,
      &FastExplorationFSM::optResMsgCallback, this, ros::TransportHints().tcpNoDelay());

  swarm_traj_pub_ = nh.advertise<bspline::Bspline>("/planning/swarm_traj_send", 100);
  swarm_traj_sub_ =
      nh.subscribe("/planning/swarm_traj_recv", 100, &FastExplorationFSM::swarmTrajCallback, this);
  swarm_traj_timer_ =
      nh.createTimer(ros::Duration(0.2), &FastExplorationFSM::swarmTrajTimerCallback, this);

  hgrid_pub_ = nh.advertise<exploration_manager::HGrid>("/swarm_expl/hgrid_send", 10);
  grid_tour_pub_ = nh.advertise<exploration_manager::GridTour>("/swarm_expl/grid_tour_send", 10);
}

int FastExplorationFSM::getId() {
  return expl_manager_->ep_->drone_id_;
}

// 状态转换、播报
void FastExplorationFSM::FSMCallback(const ros::TimerEvent& e) {
  // 每0.1s播报一次
  ROS_INFO_STREAM_THROTTLE(
      1.0, "[FSM]: Drone " << getId() << " state: " << fd_->state_str_[int(state_)]);

  switch (state_) {
    case INIT: {
      // Wait for odometry ready
      if (!fd_->have_odom_) {
        ROS_WARN_THROTTLE(1.0, "no odom");
        return;
      }
      if ((ros::Time::now() - fd_->fsm_init_time_).toSec() < 2.0) {
        ROS_WARN_THROTTLE(1.0, "wait for init");
        return;
      }
      // Go to wait trigger when odom is ok
      transitState(WAIT_TRIGGER, "FSM");
      break;
    }

    case WAIT_TRIGGER: {
      // Do nothing but wait for trigger
      ROS_WARN_THROTTLE(1.0, "wait for trigger.");
      break;
    }

    case FINISH: {
      ROS_INFO_THROTTLE(1.0, "finish exploration.");
      std_msgs::String cmd_msg;
      cmd_msg.data = "AUTO.LAND";
      land_pub_.publish(cmd_msg);
      radio_land_pub_.publish(cmd_msg);
      break;
    }

    // 处于IDLE状态过长时，返回原点
    case IDLE: {
      double check_interval = (ros::Time::now() - fd_->last_check_frontier_time_).toSec();
      if (check_interval > 10.0) {
        // if (!expl_manager_->updateFrontierStruct(fd_->odom_pos_)) {
        if (!land_instantly) {
          ROS_WARN("Drone %d, Go back to my start pos!!!", getId());
          // transitState(FINISH, "FSM");
          // break;

          expl_manager_->ed_->next_pos_ = fd_->end_pos_;
          expl_manager_->ed_->next_yaw_ = 0.0;

          // std::cout << "go back!!!" << endl;
          fd_->go_back_ = true;

          transitState(PLAN_TRAJ, "FSM");
        } else {
          ROS_WARN("Drone %d, Land Instantly!!!", getId());
          transitState(FINISH, "FSM");
        }
      }
      break;
    }
    // 计划轨迹
    case PLAN_TRAJ: {
      if (fd_->static_state_) {  // 从正常hover状态开始启动
        // Plan from static state (hover)
        fd_->start_pt_ = fd_->odom_pos_;
        fd_->start_vel_ = fd_->odom_vel_;
        fd_->start_acc_.setZero();
        fd_->start_yaw_ << fd_->odom_yaw_, 0, 0;
      } else {  // 从运动状态启动
                // Replan from non-static state, starting from 'replan_time' seconds later
        LocalTrajData* info = &planner_manager_->local_data_;
        double t_r = (ros::Time::now() - info->start_time_).toSec() + fp_->replan_time_;
        fd_->start_pt_ = info->position_traj_.evaluateDeBoorT(t_r);
        if ((fd_->start_pt_ - fd_->odom_pos_).head<2>().norm() > 1.2) {
          fd_->static_state_ = true;
          ROS_ERROR("Start point too far from odom position: %.2f",
              (fd_->start_pt_ - fd_->odom_pos_).head<2>().norm());
          transitState(PLAN_TRAJ, "FSM");
          break;
        }
        fd_->start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_r);
        fd_->start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_r);
        fd_->start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_r)[0];
        fd_->start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_r)[0];
        fd_->start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_r)[0];
      }
      // Inform traj_server the replanning
      replan_pub_.publish(std_msgs::Empty());
      int res = callExplorationPlanner();
      if (res == SUCCEED) {
        transitState(PUB_TRAJ, "FSM");
      } else if (res == FAIL) {  // Keep trying to replan
        fd_->static_state_ = true;
        std::cout << "Plan fail!!!!!" << std::endl;
      } else if (res == NO_GRID) {
        fd_->static_state_ = true;
        fd_->last_check_frontier_time_ = ros::Time::now();
        std::cout << "No grid" << endl;
        transitState(IDLE, "FSM");
        visualize(1);
        // clearVisMarker();
      }
      break;
    }

    case PUB_TRAJ: {
      double dt = (ros::Time::now() - fd_->newest_traj_.start_time).toSec();
      if (dt > 0) {
        // use the first control/sample point of the newest trajectory as the next position
        bool collision = false;
        if (!fd_->newest_traj_.pos_pts.empty()) {
          const auto& p0 = fd_->newest_traj_.pos_pts[0];
          Eigen::Vector3d next_pt(p0.x, p0.y, p0.z);
          collision = willCollide(fd_->odom_pos_, next_pt);
        }
        if (collision) {
          fd_->static_state_ = true;
          transitState(PLAN_TRAJ, "FSM");
          break;
        }
        bspline_pub_.publish(fd_->newest_traj_);
        fd_->static_state_ = false;

        // fd_->newest_traj_.drone_id = planner_manager_->swarm_traj_data_.drone_id_;
        fd_->newest_traj_.drone_id = expl_manager_->ep_->drone_id_;
        swarm_traj_pub_.publish(fd_->newest_traj_);

        thread vis_thread(&FastExplorationFSM::visualize, this, 2, false);
        vis_thread.detach();
        transitState(EXEC_TRAJ, "FSM");
      }
      break;
    }

    case EXEC_TRAJ: {
      auto tn = ros::Time::now();
      // Check whether replan is needed
      LocalTrajData* info = &planner_manager_->local_data_;
      double t_cur = (tn - info->start_time_).toSec();

      if (!fd_->go_back_) {
        bool need_replan = false;
        if (t_cur > fp_->replan_thresh2_ && expl_manager_->frontier_finder_->isFrontierCovered()) {
          std::cout << "Replan: cluster covered=====================================" << endl;
          need_replan = true;
        } else if (info->duration_ - t_cur < fp_->replan_thresh1_) {
          // Replan if traj is almost fully executed
          std::cout << "Replan: traj fully executed=================================" << endl;
          need_replan = true;
        } else if (t_cur > fp_->replan_thresh3_) {
          // Replan after some time
          std::cout << "Replan: periodic call=======================================" << endl;
          need_replan = true;
        }

        if (need_replan) {
          if (expl_manager_->updateFrontierStruct(fd_->odom_pos_) != 0) {
            // Update frontier and plan new motion
            thread vis_thread(&FastExplorationFSM::visualize, this, 1, false);
            vis_thread.detach();
            transitState(PLAN_TRAJ, "FSM");
          } else {
            // No frontier detected, finish exploration
            fd_->last_check_frontier_time_ = ros::Time::now();
            transitState(IDLE, "FSM");
            std::cout << "Idle since no frontier is detected" << endl;
            fd_->static_state_ = true;
            replan_pub_.publish(std_msgs::Empty());
            // clearVisMarker();
            visualize(1);
          }
        }
      } else {
        // Check if reach goal
        auto pos = info->position_traj_.evaluateDeBoorT(t_cur);
        if ((pos - expl_manager_->ed_->next_pos_).norm() < 0.3) {
          replan_pub_.publish(std_msgs::Empty());
          clearVisMarker();
          transitState(FINISH, "FSM");
          return;
        }
        if (t_cur > fp_->replan_thresh3_ || info->duration_ - t_cur < fp_->replan_thresh1_) {
          // Replan for going back
          replan_pub_.publish(std_msgs::Empty());
          transitState(PLAN_TRAJ, "FSM");
          thread vis_thread(&FastExplorationFSM::visualize, this, 1, false);
          vis_thread.detach();
        }
      }

      break;
    }
    case URGENT_STOP: {
      // if (!time_begin) {
      //   std_msgs::Bool msg;
      //   msg.data = true;
      //   replan_pub_.publish(std_msgs::Empty());
      //   stop_pub_.publish(msg);
      //   time_begin = true;
      //   stop_time = ros::Time::now();
      // } else {
      //   auto tn = ros::Time::now();
      //   if ((tn - stop_time).toSec() >= 1.0) {
      //     std_msgs::Bool msg;
      //     msg.data = false;
      //     stop_pub_.publish(msg);
      //     fd_->static_state_ = true;
      //     transitState(PLAN_TRAJ, "begin");
      //     time_begin = false;
      //   }
      // }
      ROS_INFO_THROTTLE(1.0, "in urgent state.");
      break;
    }

    case PLAN_TRAJ_CA: {
      auto ed = expl_manager_->ed_;
      // bool sucess = planner_manager_->kinodynamicReplan(
      //     fd_->odom_pos_, fd_->odom_vel_, fd_->start_acc_, next_goal_, Vector3d(0, 0, 0));

      bool optimistic = false;
      planner_manager_->path_finder_->reset();
      // 如果无法到达目标点，则返回失败。
      if (planner_manager_->path_finder_->search(fd_->odom_pos_, next_goal_, optimistic, false,
              ed->collision, ed->collision_pos_) != Astar::REACH_END) {
        // ROS_ERROR("No path to next viewpoint");
        break;
      }
      ed->path_next_goal_ = planner_manager_->path_finder_->getPath();
      double time_lb = 0.8;
      bool sucess = planner_manager_->planExploreTraj(
          ed->path_next_goal_, fd_->odom_vel_, fd_->start_acc_, time_lb);

      if (sucess) {

        auto info = &planner_manager_->local_data_;
        ros::Time time_r = ros::Time::now() + ros::Duration(fp_->replan_time_);
        info->start_time_ = (ros::Time::now() - time_r).toSec() > 0 ? ros::Time::now() : time_r;

        bspline::Bspline bspline;
        bspline.order = planner_manager_->pp_.bspline_degree_;
        bspline.start_time = info->start_time_;
        bspline.traj_id = info->traj_id_;
        Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
        for (int i = 0; i < pos_pts.rows(); ++i) {
          geometry_msgs::Point pt;
          pt.x = pos_pts(i, 0);
          pt.y = pos_pts(i, 1);
          pt.z = pos_pts(i, 2);
          bspline.pos_pts.push_back(pt);
        }
        Eigen::VectorXd knots = info->position_traj_.getKnot();
        for (int i = 0; i < knots.rows(); ++i) {
          bspline.knots.push_back(knots(i));
        }
        Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
        for (int i = 0; i < yaw_pts.rows(); ++i) {
          double yaw = yaw_pts(i, 0);
          bspline.yaw_pts.push_back(yaw);
        }
        bspline.yaw_dt = info->yaw_traj_.getKnotSpan();
        fd_->newest_traj_ = bspline;

        transitState(PUB_TRAJ_CA, "FSM");
        visualize(1);
      }
      break;
    }

    case PUB_TRAJ_CA: {

      double dt = (ros::Time::now() - fd_->newest_traj_.start_time).toSec();
      if (dt > 0) {
        bspline_pub_.publish(fd_->newest_traj_);
        fd_->static_state_ = false;

        // fd_->newest_traj_.drone_id = planner_manager_->swarm_traj_data_.drone_id_;
        fd_->newest_traj_.drone_id = expl_manager_->ep_->drone_id_;
        swarm_traj_pub_.publish(fd_->newest_traj_);

        thread vis_thread(&FastExplorationFSM::visualize, this, 2, false);
        vis_thread.detach();
        transitState(EXEC_TRAJ_CA, "FSM");
      }

      break;
    }

    case EXEC_TRAJ_CA: {
      auto tn = ros::Time::now();
      // Check whether replan is needed
      LocalTrajData* info = &planner_manager_->local_data_;
      double t_cur = (tn - info->start_time_).toSec();

      if (!fd_->go_back_) {
        bool need_replan = false;
        if (info->duration_ - t_cur < fp_->replan_thresh1_) {
          // Replan if traj is almost fully executed
          std::cout << "Replan: traj fully executed=================================" << endl;
          need_replan = true;
        }

        if (need_replan) {
          if (expl_manager_->updateFrontierStruct(fd_->odom_pos_) != 0) {
            // Update frontier and plan new motion
            thread vis_thread(&FastExplorationFSM::visualize, this, 1, false);
            vis_thread.detach();
            transitState(PLAN_TRAJ, "FSM");
          } else {
            // No frontier detected, finish exploration
            ftr_count_++;
            if (ftr_count_ > 5) {
              fd_->last_check_frontier_time_ = ros::Time::now();
              transitState(IDLE, "FSM");
              std::cout << "Idle since no frontier is detected" << endl;
              fd_->static_state_ = true;
              replan_pub_.publish(std_msgs::Empty());
              // clearVisMarker();
              visualize(1);
            } else {
              ROS_ERROR("No frontier detected, redetecting 5 times.");
              transitState(PLAN_TRAJ, "FSM");
            }
          }
        }
      }

      break;
    }
  }
}

/*
  用于在机器人探索过程中调用规划器，生成新的轨迹。它根据当前的状态判断是否需要重新规划轨迹，并处理规划结果。
*/
int FastExplorationFSM::callExplorationPlanner() {
  ros::Time time_r = ros::Time::now() + ros::Duration(fp_->replan_time_);

  int res;
  if (fd_->avoid_collision_ ||
      fd_->go_back_) {  // Only replan trajectory，如果开启了群体避障或需要回退到某个位置
    res = expl_manager_->planTrajToView(fd_->start_pt_, fd_->start_vel_, fd_->start_acc_,
        fd_->start_yaw_, expl_manager_->ed_->next_pos_, expl_manager_->ed_->next_yaw_);
    fd_->avoid_collision_ = false;
  } else {  // Do full planning normally
    res = expl_manager_->planExploreMotion(
        fd_->start_pt_, fd_->start_vel_, fd_->start_acc_, fd_->start_yaw_);
  }

  // 成功规划，提取轨迹的控制点、节点信息、偏航轨迹，转换为 B 样条轨迹格式并保存。
  if (res == SUCCEED) {
    auto info = &planner_manager_->local_data_;
    info->start_time_ = (ros::Time::now() - time_r).toSec() > 0 ? ros::Time::now() : time_r;

    bspline::Bspline bspline;
    bspline.order = planner_manager_->pp_.bspline_degree_;
    bspline.start_time = info->start_time_;
    bspline.traj_id = info->traj_id_;
    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
    for (int i = 0; i < pos_pts.rows(); ++i) {
      geometry_msgs::Point pt;
      pt.x = pos_pts(i, 0);
      pt.y = pos_pts(i, 1);
      pt.z = pos_pts(i, 2);
      bspline.pos_pts.push_back(pt);
    }
    Eigen::VectorXd knots = info->position_traj_.getKnot();
    for (int i = 0; i < knots.rows(); ++i) {
      bspline.knots.push_back(knots(i));
    }
    Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
    for (int i = 0; i < yaw_pts.rows(); ++i) {
      double yaw = yaw_pts(i, 0);
      bspline.yaw_pts.push_back(yaw);
    }
    bspline.yaw_dt = info->yaw_traj_.getKnotSpan();
    fd_->newest_traj_ = bspline;
  }
  return res;
}

void FastExplorationFSM::visualize(int content, bool no_trajs) {
  // content 1: frontier; 2 paths & trajs
  auto info = &planner_manager_->local_data_;
  auto plan_data = &planner_manager_->plan_data_;
  auto ed_ptr = expl_manager_->ed_;

  auto getColorVal = [&](const int& id, const int& num, const int& drone_id) {
    double a = (drone_id - 1) / double(num + 1);
    double b = 1 / double(num + 1);
    return a + b * double(id) / ed_ptr->frontiers_.size();
  };

  if (content == 1) {
    // Draw frontier
    static int last_ftr_num = 0;
    static int last_dftr_num = 0;
    for (int i = 0; i < ed_ptr->frontiers_.size(); ++i) {
      visualization_->drawCubes(ed_ptr->frontiers_[i], 0.1,
          visualization_->getColor(double(i) / ed_ptr->frontiers_.size(), 0.4), "frontier", i, 4);
    }
    for (int i = ed_ptr->frontiers_.size(); i < last_ftr_num; ++i) {
      visualization_->drawCubes({}, 0.1, Vector4d(0, 0, 0, 1), "frontier", i, 4);
      // visualization_->drawBox(Vector3d(0, 0, 0), Vector3d(0, 0, 0), Vector4d(1, 0, 0, 0.3),
      // "frontier_boxes", i, 4);
    }
    last_ftr_num = ed_ptr->frontiers_.size();

  } else if (content == 2) {

    // Hierarchical grid and global tour --------------------------------
    // vector<Eigen::Vector3d> pts1, pts2;
    // expl_manager_->uniform_grid_->getPath(pts1, pts2);
    // visualization_->drawLines(pts1, pts2, 0.05, Eigen::Vector4d(1, 0.3, 0, 1), "partition", 0,
    // 6);

    if (expl_manager_->ep_->drone_id_ == 1) {
      vector<Eigen::Vector3d> pts1, pts2;
      expl_manager_->hgrid_->getGridMarker(pts1, pts2);
      visualization_->drawLines(pts1, pts2, 0.05, Eigen::Vector4d(1, 0, 1, 0.5), "partition", 1, 6);

      vector<Eigen::Vector3d> pts;
      vector<string> texts;
      expl_manager_->hgrid_->getGridMarker2(pts, texts);
      static int last_text_num = 0;
      for (int i = 0; i < pts.size(); ++i) {
        visualization_->drawText(pts[i], texts[i], 1, Eigen::Vector4d(0, 0, 0, 1), "text", i, 6);
      }
      for (int i = pts.size(); i < last_text_num; ++i) {
        visualization_->drawText(
            Eigen::Vector3d(0, 0, 0), string(""), 1, Eigen::Vector4d(0, 0, 0, 1), "text", i, 6);
      }
      last_text_num = pts.size();
    }

    auto grid_tour = expl_manager_->ed_->grid_tour_;
    // auto grid_tour = expl_manager_->ed_->grid_tour2_;
    // for (auto& pt : grid_tour) pt = pt + trans;

    visualization_->drawLines(grid_tour, 0.05,
        PlanningVisualization::getColor(
            (expl_manager_->ep_->drone_id_ - 1) / double(expl_manager_->ep_->drone_num_)),
        "grid_tour", 0, 6);

    // Publish grid tour to ground node
    exploration_manager::GridTour tour;
    for (int i = 0; i < grid_tour.size(); ++i) {
      geometry_msgs::Point point;
      point.x = grid_tour[i][0];
      point.y = grid_tour[i][1];
      point.z = grid_tour[i][2];
      tour.points.push_back(point);
    }
    tour.drone_id = expl_manager_->ep_->drone_id_;
    tour.stamp = ros::Time::now().toSec();
    grid_tour_pub_.publish(tour);

    if (!no_trajs) {
      // visualization_->drawBspline(info->position_traj_, 0.1,
      //     PlanningVisualization::getColor(
      //         (expl_manager_->ep_->drone_id_ - 1) / double(expl_manager_->ep_->drone_num_)),
      //     true, 0.15, Vector4d(1, 1, 0, 1));
    }
  }
}

void FastExplorationFSM::endCallback(std_msgs::String msg) {
  if (fd_->go_back_ == false) {
    std_msgs::String cmd_msg;
    cmd_msg.data = "AUTO.LAND";
    land_pub_.publish(cmd_msg);
  }
}

void FastExplorationFSM::clearVisMarker() {
  for (int i = 0; i < 10; ++i) {
    visualization_->drawCubes({}, 0.1, Vector4d(0, 0, 0, 1), "frontier", i, 4);
  }
  // visualization_->drawSpheres({}, 0.2, Vector4d(0, 0.5, 0, 1), "points", 0, 6);
  visualization_->drawLines({}, 0.07, Vector4d(0, 0.5, 0, 1), "frontier_tour", 0, 6);
  visualization_->drawLines({}, 0.07, Vector4d(0, 0.5, 0, 1), "grid_tour", 0, 6);
  visualization_->drawSpheres({}, 0.1, Vector4d(0, 0, 1, 1), "B-Spline", 0, 0);
}

void FastExplorationFSM::frontierCallback(const ros::TimerEvent& e) {
  if (state_ == WAIT_TRIGGER) {
    auto ft = expl_manager_->frontier_finder_;
    auto ed = expl_manager_->ed_;

    auto getColorVal = [&](const int& id, const int& num, const int& drone_id) {
      double a = (drone_id - 1) / double(num + 1);
      double b = 1 / double(num + 1);
      return a + b * double(id) / ed->frontiers_.size();
    };

    expl_manager_->updateFrontierStruct(fd_->odom_pos_);

    cout << "odom: " << fd_->odom_pos_.transpose() << endl;
    vector<int> tmp_id1;
    vector<vector<int>> tmp_id2;
    bool status = expl_manager_->findGlobalTourOfGrid(
        { fd_->odom_pos_ }, { fd_->odom_vel_ }, tmp_id1, tmp_id2, true);

    // Draw frontier and bounding box
    for (int i = 0; i < ed->frontiers_.size(); ++i) {
      visualization_->drawCubes(ed->frontiers_[i], 0.1,
          visualization_->getColor(double(i) / ed->frontiers_.size(), 0.4), "frontier", i, 4);
    }
    for (int i = ed->frontiers_.size(); i < 50; ++i) {
      visualization_->drawCubes({}, 0.1, Vector4d(0, 0, 0, 1), "frontier", i, 4);
    }
    if (status) {
      visualize(2, true);
    } else
      visualization_->drawLines({}, 0.07, Vector4d(0, 0.5, 0, 1), "grid_tour", 0, 6);

    // Draw grid tour
  }
}

void FastExplorationFSM::triggerCallback(const geometry_msgs::PointStampedConstPtr& msg) {

  if (state_ != WAIT_TRIGGER) return;
  fd_->trigger_ = true;
  cout << "Triggered!" << endl;
  fd_->start_pos_ = fd_->odom_pos_;
  fd_->end_pos_(0) = fd_->start_pos_(0) + end_diff_;
  fd_->end_pos_(1) = fd_->start_pos_(1);
  fd_->end_pos_(2) = fd_->start_pos_(2);

  ROS_WARN_STREAM("Start expl pos: " << fd_->start_pos_.transpose());

  if (expl_manager_->updateFrontierStruct(fd_->odom_pos_) != 0) {
    transitState(PLAN_TRAJ, "triggerCallback");
  }
  // } else
  //   transitState(FINISH, "triggerCallback");
}

// void FastExplorationFSM::triggerCallback(const geometry_msgs::PoseStampedConstPtr& msg) {

//   if (state_ != WAIT_TRIGGER) return;
//   fd_->trigger_ = true;
//   cout << "Triggered!" << endl;
//   fd_->start_pos_ = fd_->odom_pos_;
//   ROS_WARN_STREAM("Start expl pos: " << fd_->start_pos_.transpose());

//   if (expl_manager_->updateFrontierStruct(fd_->odom_pos_) != 0) {
//     transitState(PLAN_TRAJ, "triggerCallback");
//   } else
//     transitState(FINISH, "triggerCallback");
// }

void FastExplorationFSM::safetyCallback(const ros::TimerEvent& e) {
  if (state_ == EXPL_STATE::EXEC_TRAJ) {
    // Check safety and trigger replan if necessary
    double dist;
    bool safe = planner_manager_->checkTrajCollision(dist);
    if (!safe) {
      std::cout << "Replan: collision detected==================================" << endl;
      // fd_->avoid_collision_ = true;
      fd_->static_state_ = true;
      transitState(PLAN_TRAJ, "safetyCallback");
    }
  }
}

void FastExplorationFSM::odometryCallback(const nav_msgs::OdometryConstPtr& msg) {
  fd_->odom_pos_(0) = msg->pose.pose.position.x;
  fd_->odom_pos_(1) = msg->pose.pose.position.y;
  fd_->odom_pos_(2) = msg->pose.pose.position.z;

  fd_->odom_vel_(0) = msg->twist.twist.linear.x;
  fd_->odom_vel_(1) = msg->twist.twist.linear.y;
  fd_->odom_vel_(2) = msg->twist.twist.linear.z;

  fd_->odom_orient_.w() = msg->pose.pose.orientation.w;
  fd_->odom_orient_.x() = msg->pose.pose.orientation.x;
  fd_->odom_orient_.y() = msg->pose.pose.orientation.y;
  fd_->odom_orient_.z() = msg->pose.pose.orientation.z;

  Eigen::Vector3d rot_x = fd_->odom_orient_.toRotationMatrix().block<3, 1>(0, 0);
  fd_->odom_yaw_ = atan2(rot_x(1), rot_x(0));

  if (!fd_->have_odom_) {
    fd_->have_odom_ = true;
    fd_->fsm_init_time_ = ros::Time::now();
  }
}

void FastExplorationFSM::transitState(EXPL_STATE new_state, string pos_call) {
  int pre_s = int(state_);
  state_ = new_state;
  ROS_INFO_STREAM("[" + pos_call + "]: Drone "
                  << getId()
                  << " from " + fd_->state_str_[pre_s] + " to " + fd_->state_str_[int(new_state)]);
}

void FastExplorationFSM::droneStateTimerCallback(const ros::TimerEvent& e) {
  // Broadcast own state periodically
  exploration_manager::DroneState msg;
  msg.drone_id = getId();
  msg.next_pos = { float(expl_manager_->ed_->next_pos_[0]), float(expl_manager_->ed_->next_pos_[1]),
    float(expl_manager_->ed_->next_pos_[2]) };

  auto& state = expl_manager_->ed_->swarm_state_[msg.drone_id - 1];

  // if (fd_->static_state_) {
  //   state.pos_ = fd_->odom_pos_;
  //   state.vel_ = fd_->odom_vel_;
  //   state.yaw_ = fd_->odom_yaw_;
  // } else {
  //   LocalTrajData* info = &planner_manager_->local_data_;
  //   double t_r = (ros::Time::now() - info->start_time_).toSec();
  //   state.pos_ = info->position_traj_.evaluateDeBoorT(t_r);
  //   state.vel_ = info->velocity_traj_.evaluateDeBoorT(t_r);
  //   state.yaw_ = info->yaw_traj_.evaluateDeBoorT(t_r)[0];
  // }
  state.pre_pos_ = state.pos_;
  state.pos_ = fd_->odom_pos_;
  state.vel_ = fd_->odom_vel_;
  state.yaw_ = fd_->odom_yaw_;
  state.pre_time_ = state.stamp_;
  state.stamp_ = ros::Time::now().toSec();
  // msg.pos = { float(state.pos_[0]), float(state.pos_[1]), float(state.pos_[2]) };
  if (state_ != FINISH) {
    msg.pos = { float(fd_->odom_pos_[0]), float(fd_->odom_pos_[1]), float(fd_->odom_pos_[2]) };
  } else {
    msg.pos = { -100.0, -100.0, 0.0 };
  }
  msg.vel = { float(state.vel_[0]), float(state.vel_[1]), float(state.vel_[2]) };
  msg.yaw = state.yaw_;
  for (auto id : state.grid_ids_) msg.grid_ids.push_back(id);
  msg.recent_attempt_time = state.recent_attempt_time_;
  msg.stamp = state.stamp_;

  drone_state_pub_.publish(msg);
}

void FastExplorationFSM::droneStateMsgCallback(const exploration_manager::DroneStateConstPtr& msg) {
  // Update other drones' states
  if (msg->drone_id == getId()) return;

  // Simulate swarm communication loss
  Eigen::Vector3d msg_pos(msg->pos[0], msg->pos[1], msg->pos[2]);
  // if ((msg_pos - fd_->odom_pos_).norm() > 6.0) return;

  auto& drone_state = expl_manager_->ed_->swarm_state_[msg->drone_id - 1];
  if (drone_state.stamp_ + 1e-4 >= msg->stamp) return;  // Avoid unordered msg

  drone_state.pre_pos_ = drone_state.pos_;
  drone_state.pos_ = Eigen::Vector3d(msg->pos[0], msg->pos[1], msg->pos[2]);
  drone_state.vel_ = Eigen::Vector3d(msg->vel[0], msg->vel[1], msg->vel[2]);
  drone_state.yaw_ = msg->yaw;
  drone_state.grid_ids_.clear();
  for (auto id : msg->grid_ids) drone_state.grid_ids_.push_back(id);
  drone_state.pre_time_ = drone_state.stamp_;
  drone_state.stamp_ = msg->stamp;
  drone_state.recent_attempt_time_ = msg->recent_attempt_time;

  // std::cout << "Drone " << getId() << " get drone " << int(msg->drone_id) << "'s state" <<
  // std::endl; std::cout << drone_state.pos_.transpose() << std::endl;
}

void FastExplorationFSM::optTimerCallback(const ros::TimerEvent& e) {
  if (state_ == INIT || state_ == FINISH) return;
  // if (state_ != WAIT_TRIGGER) return;
  //  ROS_ERROR("asdqw");

  // Select nearby drone not interacting with recently
  auto& states = expl_manager_->ed_->swarm_state_;
  auto& state1 = states[getId() - 1];
  // bool urgent = (state1.grid_ids_.size() <= 1 /* && !state1.grid_ids_.empty() */);
  bool urgent = state1.grid_ids_.empty();
  auto tn = ros::Time::now().toSec();

  // Avoid frequent attempt
  if (tn - state1.recent_attempt_time_ < fp_->attempt_interval_) return;

  int select_id = -1;
  double max_interval = -1.0;
  for (int i = 0; i < states.size(); ++i) {
    if (i + 1 <= getId()) continue;
    // Check if have communication recently
    // or the drone just experience another opt
    // or the drone is interacted with recently /* !urgent &&  */
    // or the candidate drone dominates enough grids
    if (tn - states[i].stamp_ > 0.5) {
      if (i == states.size() - 1) {
        continue;
      }
      // std::cout<<"my tn: " << tn << std::endl;
      // std::cout<<"With " << i + 1 << ": " << states[i].stamp_ << std::endl;
      continue;
    }
    if (tn - states[i].recent_attempt_time_ < fp_->attempt_interval_) {
      // std::cout<<"222"<<std::endl;
      continue;
    }
    if (tn - states[i].recent_interact_time_ < fp_->pair_opt_interval_) {
      continue;
    }
    if (states[i].grid_ids_.size() + state1.grid_ids_.size() == 0) {
      // std::cout<<"333"<<std::endl;
      continue;
    }
    double interval = tn - states[i].recent_interact_time_;
    if (interval <= max_interval) {
      // std::cout<<"444"<<std::endl;
      continue;
    }
    select_id = i + 1;
    max_interval = interval;
  }
  if (select_id == -1) return;

  // std::cout << "\nSelect: " << select_id << std::endl;
  // std::cout<<"Pair opt "<< getId() << "&" << select_id << endl;

  // Do pairwise optimization with selected drone, allocate the union of their domiance grids
  unordered_map<int, char> opt_ids_map;
  auto& state2 = states[select_id - 1];
  for (auto id : state1.grid_ids_) opt_ids_map[id] = 1;
  for (auto id : state2.grid_ids_) opt_ids_map[id] = 1;
  vector<int> opt_ids;
  for (auto pair : opt_ids_map) opt_ids.push_back(pair.first);

  // std::cout << "Pair Opt id: ";
  // for (auto id : opt_ids) std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // Find missed grids to reallocated them
  vector<int> actives, missed;
  expl_manager_->hgrid_->getActiveGrids(actives);
  findUnallocated(actives, missed);
  // std::cout << "Missed: ";
  // for (auto id : missed) std::cout << id << ", ";
  // std::cout << "" << std::endl;
  opt_ids.insert(opt_ids.end(), missed.begin(), missed.end());

  // Do partition of the grid
  vector<Eigen::Vector3d> positions = { state1.pos_, state2.pos_ };
  vector<Eigen::Vector3d> velocities = { Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0) };
  vector<int> first_ids1, second_ids1, first_ids2, second_ids2;
  if (state_ != WAIT_TRIGGER) {
    expl_manager_->hgrid_->getConsistentGrid(
        state1.grid_ids_, state1.grid_ids_, first_ids1, second_ids1);
    expl_manager_->hgrid_->getConsistentGrid(
        state2.grid_ids_, state2.grid_ids_, first_ids2, second_ids2);
  }

  auto t1 = ros::Time::now();

  vector<int> ego_ids, other_ids;
  expl_manager_->allocateGrids(positions, velocities, { first_ids1, first_ids2 },
      { second_ids1, second_ids2 }, opt_ids, ego_ids, other_ids);

  double alloc_time = (ros::Time::now() - t1).toSec();

  // std::cout << "Ego1  : ";
  // for (auto id : state1.grid_ids_) std::cout << id << ", ";
  // std::cout << "\nOther1: ";
  // for (auto id : state2.grid_ids_) std::cout << id << ", ";
  // std::cout << "\nEgo2  : ";
  // for (auto id : ego_ids) std::cout << id << ", ";
  // std::cout << "\nOther2: ";
  // for (auto id : other_ids) std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // Check results
  double prev_app1 = expl_manager_->computeGridPathCost(state1.pos_, state1.grid_ids_, first_ids1,
      { first_ids1, first_ids2 }, { second_ids1, second_ids2 }, true);
  double prev_app2 = expl_manager_->computeGridPathCost(state2.pos_, state2.grid_ids_, first_ids2,
      { first_ids1, first_ids2 }, { second_ids1, second_ids2 }, true);
  // std::cout << "prev cost: " << prev_app1 << ", " << prev_app2 << ", " << prev_app1 + prev_app2
  //           << std::endl;
  double cur_app1 = expl_manager_->computeGridPathCost(state1.pos_, ego_ids, first_ids1,
      { first_ids1, first_ids2 }, { second_ids1, second_ids2 }, true);
  double cur_app2 = expl_manager_->computeGridPathCost(state2.pos_, other_ids, first_ids2,
      { first_ids1, first_ids2 }, { second_ids1, second_ids2 }, true);
  // std::cout << "cur cost : " << cur_app1 << ", " << cur_app2 << ", " << cur_app1 + cur_app2
  //           << std::endl;
  if (cur_app1 + cur_app2 > prev_app1 + prev_app2 + 0.5) {
    // ROS_ERROR("Larger cost after reallocation");
    if (state_ != WAIT_TRIGGER) {
      return;
    }
  }

  // if (!state1.grid_ids_.empty() && !ego_ids.empty() &&
  //     !expl_manager_->hgrid_->isConsistent(state1.grid_ids_[0], ego_ids[0])) {
  //   ROS_ERROR("Path 1 inconsistent");
  // }
  // if (!state2.grid_ids_.empty() && !other_ids.empty() &&
  //     !expl_manager_->hgrid_->isConsistent(state2.grid_ids_[0], other_ids[0])) {
  //   ROS_ERROR("Path 2 inconsistent");
  // }

  // Update ego and other dominace grids
  auto last_ids2 = state2.grid_ids_;

  // Send the result to selected drone and wait for confirmation
  exploration_manager::PairOpt opt;
  opt.from_drone_id = getId();
  opt.to_drone_id = select_id;
  // opt.msg_type = 1;
  opt.stamp = tn;
  for (auto id : ego_ids) opt.ego_ids.push_back(id);
  for (auto id : other_ids) opt.other_ids.push_back(id);

  for (int i = 0; i < fp_->repeat_send_num_; ++i) opt_pub_.publish(opt);

  // ROS_WARN("Drone %d send opt request to %d, pair opt t: %lf, allocate t: %lf", getId(),
  // select_id,
  //     ros::Time::now().toSec() - tn, alloc_time);

  // Reserve the result and wait...
  auto ed = expl_manager_->ed_;
  ed->ego_ids_ = ego_ids;
  ed->other_ids_ = other_ids;
  ed->pair_opt_stamp_ = opt.stamp;
  ed->wait_response_ = true;
  state1.recent_attempt_time_ = tn;
}

void FastExplorationFSM::findUnallocated(const vector<int>& actives, vector<int>& missed) {
  // Create map of all active
  unordered_map<int, char> active_map;
  for (auto ativ : actives) {
    active_map[ativ] = 1;
  }

  // Remove allocated ones
  for (auto state : expl_manager_->ed_->swarm_state_) {
    for (auto id : state.grid_ids_) {
      if (active_map.find(id) != active_map.end()) {
        active_map.erase(id);
      } else {
        // ROS_ERROR("Inactive grid %d is allocated.", id);
      }
    }
  }

  missed.clear();
  for (auto p : active_map) {
    missed.push_back(p.first);
  }
}

void FastExplorationFSM::optMsgCallback(const exploration_manager::PairOptConstPtr& msg) {
  if (msg->from_drone_id == getId() || msg->to_drone_id != getId()) return;

  // Check stamp to avoid unordered/repeated msg
  if (msg->stamp <= expl_manager_->ed_->pair_opt_stamps_[msg->from_drone_id - 1] + 1e-4) return;
  expl_manager_->ed_->pair_opt_stamps_[msg->from_drone_id - 1] = msg->stamp;

  auto& state1 = expl_manager_->ed_->swarm_state_[msg->from_drone_id - 1];
  auto& state2 = expl_manager_->ed_->swarm_state_[getId() - 1];

  // auto tn = ros::Time::now().toSec();
  exploration_manager::PairOptResponse response;
  response.from_drone_id = msg->to_drone_id;
  response.to_drone_id = msg->from_drone_id;
  response.stamp = msg->stamp;  // reply with the same stamp for verificaiton

  if (msg->stamp - state2.recent_attempt_time_ < fp_->attempt_interval_) {
    // Just made another pair opt attempt, should reject this attempt to avoid frequent changes
    std::cout << "Reject frequent attempt" << endl;
    response.status = 2;
  } else {
    // No opt attempt recently, and the grid info between drones are consistent, the pair opt
    // request can be accepted
    response.status = 1;

    // Update from the opt result
    state1.grid_ids_.clear();
    state2.grid_ids_.clear();
    for (auto id : msg->ego_ids) state1.grid_ids_.push_back(id);
    for (auto id : msg->other_ids) state2.grid_ids_.push_back(id);

    state1.recent_interact_time_ = msg->stamp;
    state2.recent_attempt_time_ = ros::Time::now().toSec();
    expl_manager_->ed_->reallocated_ = true;

    if (state_ == IDLE && !state2.grid_ids_.empty()) {
      transitState(PLAN_TRAJ, "optMsgCallback");
      ROS_WARN("Restart after opt!");
    }

    // if (!check_consistency(tmp1, tmp2)) {
    //   response.status = 2;
    //   ROS_WARN("Inconsistent grid info, reject pair opt");
    // } else {
    // }
  }
  for (int i = 0; i < fp_->repeat_send_num_; ++i) opt_res_pub_.publish(response);
}

void FastExplorationFSM::optResMsgCallback(
    const exploration_manager::PairOptResponseConstPtr& msg) {
  if (msg->from_drone_id == getId() || msg->to_drone_id != getId()) return;

  // Check stamp to avoid unordered/repeated msg
  if (msg->stamp <= expl_manager_->ed_->pair_opt_res_stamps_[msg->from_drone_id - 1] + 1e-4) return;
  expl_manager_->ed_->pair_opt_res_stamps_[msg->from_drone_id - 1] = msg->stamp;

  auto ed = expl_manager_->ed_;
  // Verify the consistency of pair opt via time stamp
  if (!ed->wait_response_ || fabs(ed->pair_opt_stamp_ - msg->stamp) > 1e-5) return;

  ed->wait_response_ = false;
  // ROS_WARN("get response %d", int(msg->status));

  if (msg->status != 1) return;  // Receive 1 for valid opt

  auto& state1 = ed->swarm_state_[getId() - 1];
  auto& state2 = ed->swarm_state_[msg->from_drone_id - 1];
  state1.grid_ids_ = ed->ego_ids_;
  state2.grid_ids_ = ed->other_ids_;
  state2.recent_interact_time_ = ros::Time::now().toSec();
  ed->reallocated_ = true;

  if (state_ == IDLE && !state1.grid_ids_.empty()) {
    transitState(PLAN_TRAJ, "optResMsgCallback");
    ROS_WARN("Restart after opt!");
  }
}

void FastExplorationFSM::swarmTrajCallback(const bspline::BsplineConstPtr& msg) {
  // Get newest trajs from other drones, for inter-drone collision avoidance
  auto& sdat = planner_manager_->swarm_traj_data_;

  // Ignore self trajectory
  if (msg->drone_id == sdat.drone_id_) return;

  // Ignore outdated trajectory
  if (sdat.receive_flags_[msg->drone_id - 1] == true &&
      msg->start_time.toSec() <= sdat.swarm_trajs_[msg->drone_id - 1].start_time_ + 1e-3)
    return;

  // Convert the msg to B-spline
  Eigen::MatrixXd pos_pts(msg->pos_pts.size(), 3);
  Eigen::VectorXd knots(msg->knots.size());
  for (int i = 0; i < msg->knots.size(); ++i) knots(i) = msg->knots[i];

  for (int i = 0; i < msg->pos_pts.size(); ++i) {
    pos_pts(i, 0) = msg->pos_pts[i].x;
    pos_pts(i, 1) = msg->pos_pts[i].y;
    pos_pts(i, 2) = msg->pos_pts[i].z;
  }

  sdat.swarm_trajs_[msg->drone_id - 1].setUniformBspline(pos_pts, msg->order, 0.1);
  sdat.swarm_trajs_[msg->drone_id - 1].setKnot(knots);
  sdat.swarm_trajs_[msg->drone_id - 1].start_time_ = msg->start_time.toSec();
  sdat.receive_flags_[msg->drone_id - 1] = true;

  // if (state_ == EXEC_TRAJ) {
  //   // Check collision with received trajectory
  //   if (!planner_manager_->checkSwarmCollision(msg->drone_id)) {
  //     ROS_ERROR("Drone %d collide with drone %d.", sdat.drone_id_, msg->drone_id);
  //     fd_->avoid_collision_ = true;
  //     transitState(PLAN_TRAJ, "swarmTrajCallback");
  //   }
  // }
}

void FastExplorationFSM::swarmTrajTimerCallback(const ros::TimerEvent& e) {
  // Broadcast newest traj of this drone to others
  if (state_ == EXEC_TRAJ) {
    swarm_traj_pub_.publish(fd_->newest_traj_);

  } else if (state_ == URGENT_STOP || state_ == WAIT_TRIGGER) {
    // Publish a virtual traj at current pose, to avoid collision
    bspline::Bspline bspline;
    bspline.order = planner_manager_->pp_.bspline_degree_;
    bspline.start_time = ros::Time::now();
    bspline.traj_id = planner_manager_->local_data_.traj_id_;

    Eigen::MatrixXd pos_pts(4, 3);
    for (int i = 0; i < 4; ++i) pos_pts.row(i) = fd_->odom_pos_.transpose();

    for (int i = 0; i < pos_pts.rows(); ++i) {
      geometry_msgs::Point pt;
      pt.x = pos_pts(i, 0);
      pt.y = pos_pts(i, 1);
      pt.z = pos_pts(i, 2);
      bspline.pos_pts.push_back(pt);
    }

    NonUniformBspline tmp(pos_pts, planner_manager_->pp_.bspline_degree_, 2.0);
    Eigen::VectorXd knots = tmp.getKnot();
    for (int i = 0; i < knots.rows(); ++i) {
      bspline.knots.push_back(knots(i));
    }
    bspline.drone_id = expl_manager_->ep_->drone_id_;
    swarm_traj_pub_.publish(bspline);
  }
}

bool FastExplorationFSM::willCollide(
    const Eigen::Vector3d& A_pos, const Eigen::Vector3d& A_next_pos) {
  // Check other swarm states for potential collision with the next position A_next_pos.
  // Strategy: find any other drone within 2.0 meters of A_next_pos (xy distance)
  // and then use checkPositionRelativeToA(A_pos, B_pos, A_next_pos, angle, 45)
  // to decide whether that drone lies in front of A (i.e., potential collision).

  auto& states = expl_manager_->ed_->swarm_state_;

  for (int i = 0; i < (int)states.size() - 1; ++i) {
    int drone_id = i + 1;
    if (drone_id == getId()) continue;  // skip self

    const auto& state2 = states[i];
    // compute horizontal distance between A_next_pos and other drone
    double dis = (A_pos - state2.pos_).head<2>().norm();
    if (dis <= 1.2) {
      double angle = 0.0;
      bool in_front = checkPositionRelativeToA(A_pos, state2.pos_, A_next_pos, angle, 45);
      if (in_front) {
        ROS_WARN("Drone %d : Potential collision detected with Drone %d, dis: %.2f, replan again.",
            getId(), drone_id, dis);
        return true;
      }
    }
  }

  return false;
}

void FastExplorationFSM::collisionCallback(const ros::TimerEvent& e) {
  if (state_ == INIT || state_ == WAIT_TRIGGER || state_ == FINISH) return;

  if (time_begin) {
    double threshold_collision = 2.0;
    if (state_ == URGENT_STOP) {
      threshold_collision = 2.0;
    } else {
      threshold_collision = 1.0;
    }
    if ((ros::Time::now() - time_collision_freeze).toSec() >= threshold_collision) {
      time_begin = false;
      expl_manager_->ed_->collision = false;
      std_msgs::Bool msg;
      msg.data = false;
      stop_pub_.publish(msg);
      if (state_ == URGENT_STOP) {
        fd_->static_state_ = true;
        transitState(PLAN_TRAJ, "begin");
      }
    } else {
      return;
    }
  }

  auto& states = expl_manager_->ed_->swarm_state_;
  double distance = 0.0;
  double angle = 0.0;
  may_collision = false;
  may_replan = false;
  int drone = -1;
  expl_manager_->ed_->collision_pos_.clear();
  // expl_manager_->ed_->collision_pos_.push_back(states[getId() - 1].pos_);
  for (int i = 0; i < states.size() - 1; ++i) {
    if (i + 1 == getId()) {
      continue;
    } else {
      auto& state2 = states[i];
      double dis = (fd_->odom_pos_ - state2.pos_).head<2>().norm();
      if (dis <= collision_dis1_) {
        // bool avoid = calculateAngleAndIntersection(fd_->odom_pos_, state2.pos_,
        // expl_manager_->ed_->next_pos_, state2.next_pos_);
        bool avoid = riskPredict(i);
        if (avoid) {
          expl_manager_->ed_->collision_pos_.push_back(state2.pos_);
          distance = dis;
          // bool stop1 = checkPositionRelativeToA(
          //     fd_->odom_pos_, state2.pos_, expl_manager_->ed_->next_pos_, angle, angle1);
          bool stop2 = checkPositionRelativeToA_Vel(state2.pos_, angle1);
          if (stop2) {
            drone = i + 1;
            may_collision = true;
            // if (!stop1) {
            //   ROS_ERROR("Drone %d just escape, with Drone %d", getId(), i + 1);
            //   may_collision = false;
            // }
            if (may_collision) {
              break;
            }
          } else {
            drone = i + 1;
            may_replan = true;
          }
          // ROS_ERROR("Drone %d need avoid.", getId());
        }
        if (dis <= collision_dis2_) {
          bool stop2 = checkPositionRelativeToA_Vel(state2.pos_, angle2);
          if (stop2) {
            drone = i + 1;
            may_collision = true;
            break;
          } else {
            drone = i + 1;
            may_replan = true;
          }
        }
      } else {
        // states[getId() - 1].pre_pos_ = fd_->odom_pos_;
        // states[getId() - 1].pre_time_ = ros::Time::now().toSec();
        // state2.pre_pos_ = state2.pos_;
        // state2.pre_time_ = ros::Time::now().toSec();
      }
    }
  }
  if (may_collision) {
    if (state_ == EXEC_TRAJ) {
      auto& state2 = states[drone - 1];
      // fd_->avoid_collision_ = true;
      transitState(URGENT_STOP, "i got urgent");
      time_begin = true;
      time_collision_freeze = ros::Time::now();
      fd_->static_state_ = true;
      std_msgs::Bool msg;
      msg.data = true;
      replan_pub_.publish(std_msgs::Empty());
      stop_pub_.publish(msg);
      ROS_ERROR(
          "Drone %d stop, because of with Drone %d, distance: %.2f", getId(), drone, distance);
      return;
    }
  }
  if (may_replan) {
    // ROS_ERROR(
    //     "Drone %d replan, because of with Drone %d, distance: %.2f", getId(), drone, distance);
    // if (state_ == EXEC_TRAJ) {
    //   auto& state2 = states[drone - 1];
    //   transitState(PLAN_TRAJ, "i got replan");
    //   fd_->avoid_collision_ = true;
    //   fd_->static_state_ = true;
    //   time_begin = true;
    //   time_collision_freeze = ros::Time::now();
    //   expl_manager_->ed_->collision = true;
    //   ROS_ERROR("Drone %d replan", getId());

    //   return;
    // }
  }
}

bool FastExplorationFSM::checkPositionRelativeToA(const Eigen::Vector3d& A_pos,
    const Eigen::Vector3d& B_pos, const Eigen::Vector3d& A_next_pos, double& angle,
    double angle_threshold_degrees) {  // 默认45度阈值

  // 计算A的运动方向向量（忽略z轴，只考虑xy平面）
  Eigen::Vector3d A_direction = A_next_pos - A_pos;
  A_direction.z() = 0;

  // 计算从A指向B的向量（忽略z轴）
  Eigen::Vector3d A_to_B = B_pos - A_pos;
  A_to_B.z() = 0;

  // 如果A没有移动或者B与A重合，无法判断方向
  if (A_direction.norm() < 1e-10 || A_to_B.norm() < 1e-10) {
    return false;
  }

  // 归一化方向向量
  Eigen::Vector3d A_dir_normalized = A_direction.normalized();
  Eigen::Vector3d A_to_B_normalized = A_to_B.normalized();

  // 计算点积（余弦值）
  double cos_angle = A_dir_normalized.dot(A_to_B_normalized);
  angle = std::abs(cos_angle * 180 / M_PI);

  // 将角度阈值转换为弧度，计算对应的余弦值
  double angle_threshold_rad = angle_threshold_degrees * M_PI / 180.0;
  double cos_threshold = std::cos(angle_threshold_rad);

  // 如果夹角大于阈值，认为是侧方
  if (std::abs(cos_angle) < cos_threshold) {
    return false;
  }

  // 根据投影的正负判断前后关系
  double projection = A_to_B.dot(A_dir_normalized);

  if (projection > 1e-10) {
    return true;  // 正投影，B在A前方
  } else if (projection < -1e-10) {
    return false;  // 负投影，B在A后方
  } else {
    return false;  // 投影接近0，B在A侧方
  }
}

bool FastExplorationFSM::checkPositionRelativeToA_Vel(
    const Eigen::Vector3d& B_pos, double angle_threshold_degrees) {  // 默认45度阈值

  auto& states = expl_manager_->ed_->swarm_state_;
  double delta_t1 = ros::Time::now().toSec() - states[getId() - 1].pre_time_;
  Eigen::Vector3d A_pos = states[getId() - 1].pos_;

  // 计算A的运动方向向量（忽略z轴，只考虑xy平面）
  Eigen::Vector3d A_direction =
      (states[getId() - 1].pos_ - states[getId() - 1].pre_pos_) / delta_t1;

  A_direction.z() = 0;

  // 计算从A指向B的向量（忽略z轴）
  Eigen::Vector3d A_to_B = B_pos - A_pos;
  A_to_B.z() = 0;

  // 如果A没有移动或者B与A重合，无法判断方向
  if (A_direction.norm() < 1e-10 || A_to_B.norm() < 1e-10) {
    return false;
  }

  // 归一化方向向量
  Eigen::Vector3d A_dir_normalized = A_direction.normalized();
  Eigen::Vector3d A_to_B_normalized = A_to_B.normalized();

  // 计算点积（余弦值）
  double cos_angle = A_dir_normalized.dot(A_to_B_normalized);

  // 将角度阈值转换为弧度，计算对应的余弦值
  double angle_threshold_rad = angle_threshold_degrees * M_PI / 180.0;
  double cos_threshold = std::cos(angle_threshold_rad);

  // 如果夹角大于阈值，认为是侧方
  if (std::abs(cos_angle) < cos_threshold) {
    return false;
  }

  // 根据投影的正负判断前后关系
  double projection = A_to_B.dot(A_dir_normalized);

  if (projection > 1e-10) {
    return true;  // 正投影，B在A前方
  } else if (projection < -1e-10) {
    return false;  // 负投影，B在A后方
  } else {
    return false;  // 投影接近0，B在A侧方
  }
}

void FastExplorationFSM::collisionCallbackNew(const ros::TimerEvent& e) {
  if (state_ == INIT || state_ == WAIT_TRIGGER) return;

  if (time_begin) {
    double threshold_collision = 2.0;
    if (state_ == URGENT_STOP) {
      threshold_collision = 20.0;
    } else {
      threshold_collision = 10.0;
    }
    if ((ros::Time::now() - time_collision_freeze).toSec() >= threshold_collision) {
      time_begin = false;
      expl_manager_->ed_->collision = false;
      std_msgs::Bool msg;
      msg.data = false;
      stop_pub_.publish(msg);
      if (state_ == URGENT_STOP) {
        fd_->static_state_ = true;
        transitState(PLAN_TRAJ, "begin");
      }
    } else {
      return;
    }
  }
  if (state_ == URGENT_STOP) return;
  if (state_ == PLAN_TRAJ_CA || state_ == PUB_TRAJ_CA || state_ == EXEC_TRAJ_CA) return;
  auto& states = expl_manager_->ed_->swarm_state_;

  may_collision = false;
  may_replan = false;
  expl_manager_->ed_->collision_pos_.clear();
  bool collision = false;
  vector<vector<int>> vec(states.size() - 1, vector<int>(states.size() - 1, 0));
  for (int i = 0; i < states.size() - 1; ++i) {
    vec[i][i] = 1;
    for (int j = 0; j < states.size() - 1; ++j) {
      if (j == i) {
        continue;
      }
      auto& state1 = states[i];
      auto& state2 = states[j];
      double dis = (state1.pos_ - state2.pos_).head<2>().norm();
      if (dis <= 1.0) {
        collision = true;
        vec[i][j] = 1;
      }
    }
  }
  if (collision) {
    vector<int> my_cluster(states.size() - 1);
    for (int i = 0; i < states.size() - 1; ++i) {
      if (vec[getId() - 1][i] == 1) {
        my_cluster[i] = 1;
      }
    }
    vector<int> collision_cluster = findMaxUnion(my_cluster, vec);
    // for (int i = 0; i < my_cluster.size(); i++) {
    //   ROS_ERROR("%d :my_cluster: %d", getId(), my_cluster[i] + 1);
    // }
    vector<int> max_cluster;
    for (int i = 0; i < collision_cluster.size(); i++) {
      if (collision_cluster[i] == 1) {
        max_cluster.push_back(i);
      }
    }
    for (int i = 0; i < max_cluster.size(); i++) {
      ROS_ERROR("%d :max_cluster: %d", getId() - 1, max_cluster[i]);
    }

    int escape_drone_id = findMostCriticalDrone(max_cluster);
    // ROS_ERROR("%d: escape_drone_id :%d", getId(), escape_drone_id + 1);

    if (escape_drone_id + 1 == getId()) {
      ROS_ERROR("%d: escape_drone_id :%d", getId() - 1, getId() - 1);
      double step_size = 0.5;
      double best_angle_rad = calculateOptimalDirection(escape_drone_id, max_cluster, step_size);
      double best_angle_deg = best_angle_rad * 180.0 / M_PI;
      double dx = std::cos(best_angle_rad) * step_size;
      double dy = std::sin(best_angle_rad) * step_size;
      next_goal_ = Eigen::Vector3d(dx, dy, fd_->odom_pos_.z());
      ROS_ERROR("%d: next_goal : %.2f, %.2f, %.2f", getId() - 1, dx, dy, fd_->odom_pos_.z());
      transitState(PLAN_TRAJ_CA, "i got urgent");
      time_begin = true;
      time_collision_freeze = ros::Time::now();
      return;

    } else {
      ROS_ERROR("%d: i got stop : %d", getId() - 1, getId() - 1);
      transitState(URGENT_STOP, "i got urgent");
      time_begin = true;
      time_collision_freeze = ros::Time::now();
      std_msgs::Bool msg;
      msg.data = true;
      replan_pub_.publish(std_msgs::Empty());
      stop_pub_.publish(msg);
      return;
    }
  }
}

// 计算冲突的中心
std::pair<double, double> FastExplorationFSM::calculateGroupCenter(const std::vector<int>& drones) {
  double center_x = 0, center_y = 0;
  auto& states = expl_manager_->ed_->swarm_state_;
  for (const auto& drone : drones) {
    // ROS_ERROR("%d : drone: %d", getId(), drone + 1);
    center_x += states[drone].pos_.x();
    center_y += states[drone].pos_.y();
  }
  // ROS_ERROR("x:%.2f,y:%.2f", center_x / drones.size(), center_y / drones.size());

  return { center_x / drones.size(), center_y / drones.size() };
}

// 找出冲突组中最需要移动的无人机（距离组中心最远的）
int FastExplorationFSM::findMostCriticalDrone(const std::vector<int>& drones) {
  auto& states = expl_manager_->ed_->swarm_state_;
  auto center = calculateGroupCenter(drones);
  double max_distance = -1;
  int critical_drone_id = -1;

  for (const auto& drone : drones) {
    double distance = std::sqrt(
        (states[drone].pos_.x() - center.first) * (states[drone].pos_.x() - center.first) +
        (states[drone].pos_.y() - center.second) * (states[drone].pos_.y() - center.second));
    if (distance > max_distance) {
      max_distance = distance;
      critical_drone_id = drone;
    }
  }

  return critical_drone_id;
}

vector<int> FastExplorationFSM::findMaxUnion(vector<int>& my_cluster, vector<vector<int>>& vec) {
  int n = my_cluster.size();
  vector<int> result = my_cluster;  // 初始化为my_cluster
  vector<bool> visited(n, false);   // 标记节点是否被访问过

  // 遍历所有在my_cluster中为1的节点
  for (int i = 0; i < n; i++) {
    if (my_cluster[i] == 1 && !visited[i]) {
      // 使用BFS找到所有连通节点
      queue<int> q;
      q.push(i);
      visited[i] = true;

      while (!q.empty()) {
        int current = q.front();
        q.pop();

        // 遍历当前节点的所有邻居
        for (int j = 0; j < n; j++) {
          if (vec[current][j] == 1 && !visited[j]) {
            visited[j] = true;
            result[j] = 1;  // 将连通节点加入结果
            q.push(j);
          }
        }
      }
    }
  }

  return result;
}

// 计算无人机的最佳逃离方向，考虑所有相关的无人机和障碍物
double FastExplorationFSM::calculateOptimalDirection(
    int drone, const std::vector<int>& all_drones, double step_size) {
  auto& states = expl_manager_->ed_->swarm_state_;

  double x0 = states[drone].pos_.x();
  double y0 = states[drone].pos_.y();

  int num_directions = 360;
  double best_direction_rad = 0;
  double min_cost = std::numeric_limits<double>::max();

  for (int angle_deg = 0; angle_deg < num_directions; angle_deg += 5) {
    double angle_rad = angle_deg * M_PI / 180.0;
    double x_new = x0 + step_size * std::cos(angle_rad);
    double y_new = y0 + step_size * std::sin(angle_rad);

    // 计算成本函数
    double cost = 0.0;

    // 1. 与相关无人机的距离成本
    for (const auto& other : all_drones) {
      if (other == drone) {
        continue;
      }
      double dist = std::sqrt((x_new - states[other].pos_.x()) * (x_new - states[other].pos_.x()) +
                              (y_new - states[other].pos_.y()) * (y_new - states[other].pos_.y()));
      if (dist < 1e-5) {
        cost += 1000;
      } else {
        cost += 1.0 / (dist * dist);
      }
    }

    // 2. 与障碍物的距离成本
    // for (const auto& obs : obstacles) {
    //   double dist = obs.distanceTo(x_new, y_new);
    //   if (dist < SAFE_DISTANCE) {
    //     cost += (SAFE_DISTANCE - dist) * 20.0;  // 障碍物成本更高
    //   }
    // }

    // 3. 边界成本
    if (x_new <= x_min_ || x_new >= x_max_ || y_new <= y_min_ || y_new >= y_max_) {
      cost += 10000000.0;  // 高成本惩罚边界外移动
    }

    if (cost < min_cost) {
      min_cost = cost;
      best_direction_rad = angle_rad;
    }
  }

  return best_direction_rad;
}

bool FastExplorationFSM::riskPredict(int drone_id) {

  auto& states = expl_manager_->ed_->swarm_state_;
  double tau = 0.5;
  double w_d = 2.0, w_theta = 1.0, w_v = 1.0;
  double dis_safe, v_max;
  dis_safe = 1.5, v_max = 1.5;

  Vector3d r = states[drone_id].pos_ - states[getId() - 1].pos_;
  double d = r.norm();
  Vector3d hat_r = r.normalized();

  // 计算相对速度（基于位置变化）

  double delta_t1 = ros::Time::now().toSec() - states[getId() - 1].pre_time_;
  // ROS_ERROR("Drone %d: pre_time:%.2f", getId(), states[getId()].pre_time_);
  double delta_t2 = ros::Time::now().toSec() - states[drone_id].pre_time_;

  Vector3d v2 = (states[drone_id].pos_ - states[drone_id].pre_pos_) / delta_t2;
  // ROS_ERROR("Drone %d: pre_pos:%.2f", drone_id, states[drone_id].pre_pos_);
  // ROS_ERROR("Drone %d: pos:%.2f", drone_id, states[drone_id].pos_.x());

  Vector3d v1 = (states[getId() - 1].pos_ - states[getId() - 1].pre_pos_) / delta_t1;

  Vector3d v_rel = v2 - v1;
  double v_rel_mag = v_rel.norm();
  Vector3d hat_v_rel = v_rel.normalized();

  // 计算距离风险
  double hat_r_d = clip((dis_safe - d) / dis_safe, 0.0, 1.0);

  // 计算方向风险
  double cos_theta = hat_r.dot(hat_v_rel);
  // 确保cos_theta在有效范围内[-1, 1]
  cos_theta = clip(cos_theta, -1.0, 1.0);
  double hat_r_theta = (1 - cos_theta) / 2;

  // 计算速度风险
  double hat_r_v = clip(v_rel_mag / v_max, 0.0, 1.0);

  // 计算总风险
  double R = w_d * hat_r_d + w_theta * hat_r_theta + w_v * hat_r_v;
  // ROS_ERROR("R:%.2f", R);
  if (R >= tau) {
    return true;
  } else {
    return false;
  }
}

double FastExplorationFSM::clip(double value, double min_val, double max_val) {
  return std::max(min_val, std::min(value, max_val));
}

bool FastExplorationFSM::calculateAngleAndIntersection(const Eigen::Vector3d& A,
    const Eigen::Vector3d& B, Eigen::Vector3d next_pos_A, Eigen::Vector3d next_pos_B) {

  double a1 = next_pos_A.x() - A.x();
  double a2 = next_pos_A.y() - A.y();
  double b1 = next_pos_B.x() - B.x();
  double b2 = next_pos_B.y() - B.y();

  Vector3d vecA(a1, a2, 0);
  Vector3d vecB(b1, b2, 0);

  double dotProduct = vecA.dot(vecB);
  double magA = vecA.norm();
  double magB = vecB.norm();

  if (magA < 1e-10 || magB < 1e-10) {
    throw std::invalid_argument("Zero vector encountered, angle undefined.");
  }

  double cosTheta = dotProduct / (magA * magB);
  // 手动实现 clamp 功能
  if (cosTheta > 1.0)
    cosTheta = 1.0;
  else if (cosTheta < -1.0)
    cosTheta = -1.0;

  double angle = std::acos(cosTheta);

  // if (std::abs(A.z() - B.z()) > 1e-7) {
  //   return std::make_tuple(angle, Eigen::Vector3d(0, 0, 0), false, false);
  // }

  double D = a1 * (-b2) - (-b1) * a2;
  if (std::abs(D) < 1e-10) {
    return false;
  }

  double Dt = (B.x() - A.x()) * (-b2) - (B.y() - A.y()) * (-b1);
  double Ds = a1 * (B.y() - A.y()) - a2 * (B.x() - A.x());

  double t = Dt / D;
  Eigen::Vector3d intersection = A + t * vecA;
  intersection.z() = A.z();

  double vec_b1 = intersection.x() - B.x();
  double vec_b2 = intersection.y() - B.y();
  double vec_a1 = intersection.x() - A.x();
  double vec_a2 = intersection.y() - A.y();
  bool cross_a, cross_b;
  if (a1 != 0) {
    cross_a = ((vec_a1 / a1) > 0);
  } else {
    cross_a = ((vec_a2 / a2) > 0);
  }
  if (b1 != 0) {
    cross_b = ((vec_b1 / b1) > 0);
  } else {
    cross_b = ((vec_b2 / b2) > 0);
  }
  bool cross = cross_a & cross_b;
  double b_a_x = B.x() - A.x();
  double b_a_y = B.y() - A.y();

  if ((b_a_x > 0 && b_a_y > 0) || (b_a_x > 0 && b_a_y < 0)) {
    if (cross) {
      return true;
    } else {
      return false;
    }
  } else {
    if (cross_b) {
      return false;
    } else {
      return true;
    }
  }
}

void FastExplorationFSM::countCallback(const ros::TimerEvent& e) {
  if (state_ == INIT || state_ == WAIT_TRIGGER) return;
  auto& states = expl_manager_->ed_->swarm_state_;

  for (int i = 0; i < states.size() - 1; ++i) {
    if (i + 1 == getId()) {
      continue;
    } else {
      auto& state2 = states[i];
      double dis = (fd_->odom_pos_ - state2.pos_).head<3>().norm();
      if (dis <= 0.7) {
        ROS_ERROR("Drone %d with Drone %d may collision!!! distance : %.2f", getId(), i + 1, dis);
      }
    }
  }
}
void FastExplorationFSM::envCallback(const ros::TimerEvent& e) {
  if (state_ == INIT || state_ == WAIT_TRIGGER) return;
  if (time_begin_env) {
    double threshold_collision = 2.0;
    if ((ros::Time::now() - time_collision_freeze_env).toSec() >= threshold_collision) {
      time_begin = false;
      expl_manager_->ed_->collision = false;
      std_msgs::Bool msg;
      msg.data = false;
      stop_pub_.publish(msg);
      if (state_ == URGENT_STOP) {
        fd_->static_state_ = true;
        transitState(PLAN_TRAJ, "begin");
      }
    } else {
      return;
    }
  }
  double resolution_ = 0.6;
  Eigen::Vector3d cur_pos = fd_->odom_pos_;
  Eigen::Vector3d nbr_pos;
  Eigen::Vector3d step;
  double distance;
  bool occupied = false;
  for (double dx = -resolution_; dx <= resolution_ + 1e-3; dx += resolution_) {
    for (double dy = -resolution_; dy <= resolution_ + 1e-3; dy += resolution_) {
      for (double dz = -resolution_; dz <= resolution_ + 1e-3; dz += resolution_) {
        step << dx, dy, dz;
        if (step.norm() < 1e-3) continue;
        nbr_pos = cur_pos + step;

        if (expl_manager_->sdf_map_->getInflateOccupancy(nbr_pos) == 1) {
          distance = step.norm();
          occupied = true;
          break;
        }
      }
      if (occupied) break;
    }
    if (occupied) break;
  }
  if (occupied) {
    ROS_ERROR("Drone %d is too close to obstacles! dis: %.2f", getId(), distance);
    if (state_ != URGENT_STOP) {
      transitState(URGENT_STOP, "too close to obstacles");
      time_begin_env = true;
      time_collision_freeze_env = ros::Time::now();
      std_msgs::Bool msg;
      msg.data = true;
      replan_pub_.publish(std_msgs::Empty());
      stop_pub_.publish(msg);
      return;
    }
  }
}  // namespace fast_planner
}  // namespace fast_planner